#include <ros/ros.h>
#include <pcl_ros/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/registration/icp.h>
#include <pcl/registration/transforms.h>
#include <pcl/filters/passthrough.h>
#include <pcl/filters/voxel_grid.h>
#include <string>

#define POINTLIMIT 1e10

typedef pcl::PointXYZRGB PointType;
typedef pcl::PointCloud<PointType> kinectPCL;

kinectPCL::Ptr currentCloud(new kinectPCL);
double VoxelRes = 0.01;
int scanNum = 0;

void DownSample(kinectPCL::Ptr& cloud, kinectPCL::Ptr& downsampled, double res) {
	pcl::VoxelGrid<PointType> grid;
	grid.setInputCloud(cloud);
	grid.setLeafSize(res, res, res);
	grid.filter(*downsampled);
}

int getNPoints(kinectPCL::Ptr& cloud) {
	return (int)(cloud->end() - cloud->begin());
}

void PointsCallback(const kinectPCL::ConstPtr& scan) {
	kinectPCL::Ptr filteredCloud(new kinectPCL);
	pcl::PassThrough<PointType> NaNFilter;
	NaNFilter.setInputCloud(scan);
	NaNFilter.setFilterFieldName("x");
	NaNFilter.setFilterLimits(-POINTLIMIT, POINTLIMIT);
	NaNFilter.filter(*filteredCloud);
	kinectPCL::Ptr downsampled(new kinectPCL);
	DownSample(filteredCloud, currentCloud, VoxelRes);
	//printf("NPoints = %i \n", getNPoints(currentCloud));
}

int main(int argc, char** argv) {
    ros::init(argc, argv, "downsample");
    ros::NodeHandle n;
	if (argc < 3) {
		fprintf(stderr, "ERROR: Did not specify fromCloud and toCloud in \"args\"");
		return 1;
	}
	char* fromCloud = argv[1];
	char* toCloud = argv[2];
	if (argc > 3)
		VoxelRes = atof(argv[3]);
    
    printf("VoxelRes = %g\n", VoxelRes);
    
    ros::Subscriber rfidSub = n.subscribe<kinectPCL>(fromCloud, 2, PointsCallback);
    ros::Publisher pub = n.advertise<kinectPCL>(toCloud, 2);
    ros::Rate loop_rate(2);

    while(n.ok()){
        ros::spinOnce();
        if (getNPoints(currentCloud) == 0)
        	continue;
    	//printf("Sending out %i points on scan %i\n", getNPoints(currentCloud), scanNum);
        pub.publish(currentCloud);
        //printf("Finished sending points\n");
        loop_rate.sleep();
        scanNum++;
    }
    return 0;
}
